﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;

namespace Hellquest
{
    public class OBB
    {
        public Vector2[] Corners { get; private set; }
        public Vector2[] LocalAxis { get; private set; }
        private float[] Center;

        public Vector2 Position
        {
            get { return new Vector2(Center[0], Center[1]); }
            set
            {
                MoveTo(value);
            }
        }


        public OBB(Vector2 _center, float _width, float _height, float _rotation)
        {
            Vector2 xDimension = new Vector2((float)Math.Cos(_rotation), (float)Math.Sin(_rotation));
            Vector2 yDimension = new Vector2((float)-Math.Sin(_rotation), (float)Math.Cos(_rotation));

            xDimension *= _width / 2;
            yDimension *= _height / 2;

            Corners = new Vector2[4];
            Corners[0] = _center - xDimension - yDimension; //Top Left
            Corners[1] = _center - xDimension + yDimension; //Bottom Left
            Corners[2] = _center + xDimension + yDimension; //Bottom Right
            Corners[3] = _center + xDimension - yDimension; //Top Right

            UpdateAxis();
        }

        /// <summary>
        /// Intersection test using Seperating Axis Theorem
        /// </summary>
        public bool Intersects(OBB _other, ref Vector2 _projection)
        {
            Vector2 shortestAxis = Vector2.Zero;
            float shortestProjection = float.MaxValue;
            
            //this-other test
            for (int i = 0; i < 2; i++)
            {
                float t1 = Vector2.Dot(_other.Corners[0], LocalAxis[i]);

                float t1Max = t1;
                float t1Min = t1;

                for (int j = 1; j < 4; j++)
                {
                    t1 = Vector2.Dot(_other.Corners[j], LocalAxis[i]);

                    if (t1 < t1Min)
                        t1Min = t1;
                    else if (t1 > t1Max)
                        t1Max = t1;
                }

                float t2 = Vector2.Dot(Corners[0], LocalAxis[i]);

                float t2Max = t2;
                float t2Min = t2;

                for (int j = 1; j < 4; j++)
                {
                    t2 = Vector2.Dot(Corners[j], LocalAxis[i]);

                    if (t2 < t2Min)
                        t2Min = t2;
                    else if (t2 > t2Max)
                        t2Max = t2;
                }


                if (!(t1Min < t2Max && t1Max > t2Min))
                {
                    return false;
                }

                if (t2Max - t1Min > t1Max - t2Min)
                {
                    float d = t1Max - t2Min;
                    if (Math.Abs(d) < Math.Abs(shortestProjection))
                    {
                        shortestProjection = d;
                        shortestAxis = LocalAxis[i];
                    }
                }
                else
                {
                    float d = -(t2Max - t1Min);
                    if (Math.Abs(d) < Math.Abs(shortestProjection))
                    {
                        shortestProjection = d;
                        shortestAxis = LocalAxis[i];
                    }
                }
            }

            //other-this test
            for (int i = 0; i < 2; i++)
            {
                float t1 = Vector2.Dot(_other.Corners[0], _other.LocalAxis[i]);

                float t1Max = t1;
                float t1Min = t1;

                for (int j = 1; j < 4; j++)
                {
                    t1 = Vector2.Dot(_other.Corners[j], _other.LocalAxis[i]);

                    if (t1 < t1Min)
                        t1Min = t1;
                    else if (t1 > t1Max)
                        t1Max = t1;
                }

                float t2 = Vector2.Dot(Corners[0], _other.LocalAxis[i]);

                float t2Max = t2;
                float t2Min = t2;

                for (int j = 1; j < 4; j++)
                {
                    t2 = Vector2.Dot(Corners[j], _other.LocalAxis[i]);

                    if (t2 < t2Min)
                        t2Min = t2;
                    else if (t2 > t2Max)
                        t2Max = t2;
                }



                if (!(t1Min < t2Max && t1Max > t2Min))
                {
                    return false;
                }

                if (!(t1Min < t2Max && t1Max > t2Min))
                {
                    return false;
                }

                if (t2Max - t1Min > t1Max - t2Min)
                {
                    float d = t1Max - t2Min;
                    if (Math.Abs(d) < Math.Abs(shortestProjection))
                    {
                        shortestProjection = d;
                        shortestAxis = _other.LocalAxis[i];
                    }
                }
                else
                {
                    float d = -(t2Max - t1Min);
                    if (Math.Abs(d) < Math.Abs(shortestProjection))
                    {
                        shortestProjection = d;
                        shortestAxis = _other.LocalAxis[i];
                    }
                }
            }
            
            _projection = shortestAxis * shortestProjection;
            return true;
        }

        public void Offset(Vector2 _offset)
        {
            for (int i = 0; i < 4; i++)
            {
                Corners[i] += _offset;
            }
            UpdateAxis();
        }

        private void UpdateAxis()
        {
            LocalAxis = new Vector2[2];
            LocalAxis[0] = Corners[3] - Corners[0];
            LocalAxis[1] = Corners[1] - Corners[0];

            Center = new float[2];
            LocalAxis[0].Normalize(); // /= LocalAxis[0].LengthSquared();
            LocalAxis[1].Normalize(); // /= LocalAxis[1].LengthSquared();
            Center[0] = Vector2.Dot(Corners[0], LocalAxis[0]);
            Center[1] = Vector2.Dot(Corners[0], LocalAxis[1]);
        }

        private void MoveTo(Vector2 _position)
        {
            Vector2 translation = _position - Position;

            for (int i = 0; i < 4; i++)
            {
                Corners[i] += translation;
            }
            UpdateAxis();
        }
    }
}
